#include "geometry_msgs/msg/twist.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/joy.hpp"
double dead_zone(double x, double limit)
{
	limit = limit > 0 ? limit : - limit;
	return x > 0 ? (x > limit ? x : 0) : (x < -1 * limit ? x : 0);
}

/**
 * @brief Ros message Joy to Ros message Twist
 *
 */
class joynode : public rclcpp::Node
{
  private:
	rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub;
	rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub;
	rclcpp::TimerBase::SharedPtr timer;
	geometry_msgs::msg::Twist twist;
	// messages are published every time the xbox input changes
	void joy_callback(sensor_msgs::msg::Joy msg)
	{
		if (msg.axes.size() < 8 || msg.buttons.size() < 8) return;
		// in high speed mode if left trigger (axes[5]) is pressed
		double scale = (msg.axes[5] + 2) / 3;
		msg.axes[1] = dead_zone(msg.axes[1], 0.2);
		msg.axes[2] = dead_zone(msg.axes[2], 0.2);

		twist.linear.x = -1 * msg.axes[1] * scale; // moving forward when x is positive
		twist.linear.y = 0;
		twist.linear.z = 0;
		twist.angular.x = 0;
		twist.angular.y = 0;
		twist.angular.z = -1 * msg.axes[2] * scale; // turn left when z is positive
	}

	// publish cmd_vel
	void timer_callback()
	{
		this->twist_pub->publish(twist);
		RCLCPP_DEBUG(this->get_logger(), "linear %lf, rotation %lf", twist.linear.x,
					 twist.angular.z);
	}

  public:
	joynode(std::string name) : Node(name)
	{
		this->joy_sub = this->create_subscription<sensor_msgs::msg::Joy>(
			"xbox_joy", 1, std::bind(&joynode::joy_callback, this, std::placeholders::_1));
		this->twist_pub = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
		this->timer = this->create_wall_timer(std::chrono::milliseconds(100),
											  std::bind(&joynode::timer_callback, this));
	}
};
int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node = std::make_shared<joynode>("joyinput");
	rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}
